
/* Private includes ----------------------------------------------------------*/
#include "robot_ctrl.h"
#include "KF.h"
#include "functions.h"
#include "imu_temp_ctrl.h"
#include "motor_ctrl.h"
#include "pid.h"
#include "user_lib.h"
#include "vmc.h"
/* Private define ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
float aim_left_L = 0.2f;
float aim_left_R = 0.2f;
float P;
float D;
/* Private function prototypes -----------------------------------------------*/
void Robot_Ctrl(void)
{
  Robot_Get(&robot, pitch, yaw, roll, gyro[0], gyro[1], gyro[2], right_w.real_angle, left_w.real_angle, right_w.speed, left_w.speed);
  // right left
  Five_Bar_Forward_kinematic(&right_foot, right_f.real_angle, right_b.real_angle);
  Five_Bar_Forward_kinematic(&left_foot, left_f.real_angle, left_b.real_angle);
  // theta  d_theta  phi0  d_phi0  x  d_x=speed
  State_Data_Cal(&right_foot, &left_foot, &robot);
  Kalman_Cal(right_foot.delta_phi, right_foot.delta_theta, left_foot.delta_phi, left_foot.delta_theta, robot.speed);
  // s
  Fhan_update(aim_left_R, &right_fhan.x1, &right_fhan.x2, 0.01f, 0.01f, 0.001f);
  Fhan_update(aim_left_L, &left_fhan.x1, &left_fhan.x2, 0.01f, 0.01f, 0.001f);
  //
  PI_Param_Init(&PI_right_foot, 1000.0f, 0.0f, 100.0f, 25.0f, 50.0f);
  PI_Param_Init(&PI_left_foot, 500.0f, 0.0f, 100.0f, 25.0f, 50.0f);
  PI_Cal(&PI_right_foot, right_fhan.x1, right_foot.L0);
  PI_Cal(&PI_left_foot, left_fhan.x1, left_foot.L0);
  // LQR ->vmc.T
  right_vmc.F = PI_right_foot.result;
  left_vmc.F  = PI_left_foot.result;

  Vmc_Cal(&right_foot, &right_vmc);
  Vmc_Cal(&left_foot, &left_vmc);

  // Set_current(&right_f, &right_b, &right_vmc,RIGHT);
  Set_current(&left_f, &left_b, &left_vmc, LEFT);
}
